#include "point_display.h"
#include <iostream>
#include <X11/Xlib.h> // 包含 Xlib 头文件
#include <pcl/visualization/point_cloud_color_handlers.h>

// 构造函数
PointDisplay::PointDisplay() : cloud(new pcl::PointCloud<pcl::PointXYZI>), cloudId("cloud") {
    // 初始化 X 库的多线程支持
    if (!XInitThreads()) {
        std::cerr << "Failed to initialize Xlib threading support." << std::endl;
    }
    viewer = std::make_shared<pcl::visualization::PCLVisualizer>("Point Cloud Viewer");
    viewer->setBackgroundColor(0, 0, 0);
    viewer->addCoordinateSystem(1.0);
    viewer->initCameraParameters();

    // 调整相机视角为正视图
    viewer->setCameraPosition(0, 0, 10, 0, 0, 0, 0, 1, 0);
    viewer->setCameraClipDistances(0.01, 100);

}

// 析构函数
PointDisplay::~PointDisplay() {
    viewer->close();
}

// 更新点云数据
void PointDisplay::updatePointCloud(const std::vector<Points>& points) {
    std::lock_guard<std::mutex> lock(mtx); // 加锁
    if (points.empty()) {
        std::cerr << "Warning: Empty point cloud data received." << std::endl;
        return;
    }
    cloud->clear();
    for (const auto& point : points) {
        pcl::PointXYZI p;
        p.x = point.X;
        p.y = point.Y;
        p.z = point.Z;
        p.intensity = static_cast<float>(point.kind);
        cloud->push_back(p);
    }
}

// 显示点云
void PointDisplay::display() {
    std::lock_guard<std::mutex> lock(mtx); // 加锁
    pcl::PointCloud<pcl::PointXYZI>::ConstPtr const_cloud = boost::const_pointer_cast<const pcl::PointCloud<pcl::PointXYZI>>(cloud);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    colored_cloud->resize(const_cloud->size());

    for (size_t i = 0; i < const_cloud->size(); ++i) {
        pcl::PointXYZRGB colored_point;
        colored_point.x = const_cloud->points[i].x;
        colored_point.y = const_cloud->points[i].y;
        colored_point.z = const_cloud->points[i].z;
        if (const_cloud->points[i].intensity > 0) {
            colored_point.r = 255; // 强度大于 128 的点设为红色
            colored_point.g = 0;
            colored_point.b = 0;
        } else {
            colored_point.r = 0;
            colored_point.g = 255;
            colored_point.b = 0; // 强度小于等于 128 的点设为绿色
        }
        colored_cloud->points[i] = colored_point;
    }

    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb_handler(colored_cloud);

    if (viewer->updatePointCloud<pcl::PointXYZRGB>(colored_cloud, rgb_handler, "cloud")) {
        viewer->spinOnce(500); // 增加时间间隔到 500 毫秒
    } else {
        viewer->addPointCloud<pcl::PointXYZRGB>(colored_cloud, rgb_handler, "cloud");
        viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud");
    }
}

// 检查可视化窗口是否关闭
bool PointDisplay::wasStopped() {
    return viewer->wasStopped();
}    